import can
import struct
import sys
import time
import threading

class CANCommunicator:
    """
    一个用于处理特定抓果车项目CAN通信的类。
    【多线程优化版】: 通过共享数据和锁与视觉线程解耦，安全地在独立线程中运行。
    """
    # --- 协议常量定义 ---
    PROTOCOL_ID = 0x30
    REQUEST_FRUIT_DATA = bytes([0x22] * 8)
    PERSON_ALERT_DATA = bytes([0x33] * 8)

    def __init__(self, shared_data: dict, lock: threading.Lock, 
                 interface: str = 'socketcan', channel: str = 'can0', bitrate: int = 500000):
        """
        初始化CAN通信器的配置。
        """
        self.bus = None
        self.interface = interface
        self.channel = channel
        self.bitrate = bitrate
        
        # --- 核心修改：保存对共享资源和锁的引用 ---
        self.shared_data = shared_data
        self.data_lock = lock
        
        # 用于判断是否发送过"无人"状态，避免在无人时重复发送0坐标
        self._is_last_known_person_state = False
        
        # 【新增】线程控制标志
        self._running = False
        self._thread = None

    def start(self) -> bool:
        """
        启动并连接到CAN总线。
        """
        try:
            self.bus = can.interface.Bus(
                channel=self.channel,
                bustype=self.interface,
                bitrate=self.bitrate,
                receive_own_messages=False
            )
            print(f"成功连接到CAN总线: {self.bus.channel_info}")
            return True
        except can.CanError as e:
            print(f"错误：连接CAN总线失败: {e}", file=sys.stderr)
            print("提示：请确保硬件已连接，并在系统中正确配置接口。", file=sys.stderr)
            return False

    def _calculate_euclidean_distance(self, coords):
        """
        计算欧氏距离（从原点到目标点）
        
        参数:
            coords: 坐标字典 {'x': x, 'y': y, 'z': z} 或元组 (x, y, z)
        返回:
            float: 欧氏距离（单位：毫米）
        """
        if isinstance(coords, dict):
            x, y, z = coords.get('x', 0), coords.get('y', 0), coords.get('z', 0)
        else:
            x, y, z = coords if len(coords) >= 3 else (0, 0, 0)
        
        # 计算3D欧氏距离: √(x² + y² + z²)
        distance = (x**2 + y**2 + z**2)**0.5
        return distance

    def send_fruit_position_response(self, x: float, y: float, z: float):
        """
        根据协议，打包并发送水果位置的响应帧。
        """
        if self.bus is None:
            return
            
        try:
            packer = struct.Struct('<Bxhhh')
            x_int, y_int, z_int = int(round(x)), int(round(y)), int(round(z))
            can_data = packer.pack(0x08, x_int, y_int, z_int)
            
            message = can.Message(
                arbitration_id=self.PROTOCOL_ID,
                data=can_data,
                is_extended_id=False
            )
            self.bus.send(message)
            print(f"响应果位置: ID={hex(self.PROTOCOL_ID)}, Coords(X,Y,Z)={x_int, y_int, z_int}")
        except struct.error as e:
            print(f"错误：坐标打包失败。确保值在-32768到32767之间。Error: {e}", file=sys.stderr)
        except can.CanError as e:
            print(f"错误：发送CAN消息失败: {e}", file=sys.stderr)

    def send_person_alert(self, person_coords=None):
        """
        根据协议，发送"有人进入"的报警帧。
        只有当人员距离在3米（3000毫米）以内时才发送警报。
        
        参数:
            person_coords: 人员坐标，如果不提供则从共享数据中获取
        """
        if self.bus is None:
            return False
        
        # 获取人员坐标
        if person_coords is None:
            with self.data_lock:
                person_coords = self.shared_data.get('person', {'x': 0, 'y': 0, 'z': 0})
        
        # 计算欧氏距离
        distance = self._calculate_euclidean_distance(person_coords)
        
        # 只有在3米（3000毫米）以内才发送警报
        if distance > 0 and distance <= 3000:
            try:
                message = can.Message(
                    arbitration_id=self.PROTOCOL_ID,
                    data=self.PERSON_ALERT_DATA,
                    is_extended_id=False
                )
                self.bus.send(message)
                print(f"发送人员报警信号 - 距离: {distance:.1f}mm")
                return True
            except can.CanError as e:
                print(f"错误：发送报警帧失败: {e}", file=sys.stderr)
                return False
        else:
            # 距离超出警报范围，不发送
            if distance > 3000:
                # 可选：打印调试信息
                # print(f"人员距离{distance:.1f}mm超出警报范围(3000mm)，不发送警报")
                pass
            return False


    def listen_and_process(self):
        """
        持续监听总线，并根据共享数据处理消息。
        """
        if not self.bus:
            print("错误：无法监听，总线未启动。", file=sys.stderr)
            return
        
        print("CAN通信线程已启动，开始监听和处理...")
        last_alert_time = 0
        
        while self._running:
            try:
                # --- 1. 主动报警逻辑 (基于共享数据和距离判断) ---
                with self.data_lock:
                    person_coords = self.shared_data.get('person', {'x': 0, 'y': 0, 'z': 0})
                
                # 计算距离并判断是否需要发送警报
                distance = self._calculate_euclidean_distance(person_coords)
                is_person_in_alert_range = (distance > 0 and distance <= 3000)
                
                # 状态机：仅在人员进入警报范围时发送
                if is_person_in_alert_range:
                    if time.time() - last_alert_time > 0.1:  # 每100ms发送一次
                        if self.send_person_alert(person_coords):
                            last_alert_time = time.time()
                    self._is_last_known_person_state = True
                elif self._is_last_known_person_state:
                    print("人员已离开警报区域")
                    self._is_last_known_person_state = False

                # --- 2. 被动响应逻辑 ---
                message = self.bus.recv(timeout=0.01)
                
                if message is not None:
                    if message.arbitration_id == self.PROTOCOL_ID and message.data == self.REQUEST_FRUIT_DATA:
                        print("收到果位置请求...")
                        
                        with self.data_lock:
                            fruit_coords = self.shared_data.get('durian', {'x': 0, 'y': 0, 'z': 0})
                            if isinstance(fruit_coords, dict):
                                x, y, z = fruit_coords['x'], fruit_coords['y'], fruit_coords['z']
                            else:
                                x, y, z = fruit_coords
                        
                        if not (x == 0 and y == 0 and z == 0):
                            self.send_fruit_position_response(x, y, z)
                        else:
                            self.send_fruit_position_response(0.0, 0.0, 0.0)
                            print("警告：收到了请求，但没有可用的果坐标，已回复(0,0,0)。")
                            
            except Exception as e:
                if self._running:
                    print(f"CAN通信处理错误: {e}", file=sys.stderr)
                time.sleep(0.01)

    
    # 【新增】启动线程方法
    def start_thread(self):
        """
        在独立线程中启动CAN通信
        """
        if not self.start():
            return False
            
        self._running = True
        self._thread = threading.Thread(target=self.listen_and_process, name="CAN-Communication")
        self._thread.daemon = True
        self._thread.start()
        print("CAN通信线程已启动")
        return True
    
    # 【新增】停止线程方法
    def stop_thread(self):
        """
        停止CAN通信线程
        """
        self._running = False
        if self._thread and self._thread.is_alive():
            self._thread.join(timeout=2.0)
            print("CAN通信线程已停止")
        self.shutdown()
    
    def shutdown(self):
        """
        安全地关闭CAN总线连接。
        """
        self._running = False
        if self.bus:
            self.bus.shutdown()
            print("CAN总线已关闭。")
            self.bus = None
